#include "adapt_notch.h"
#include <math.h>

const double NotchFilter::BASIC_FREQUENCY[NotchFilter::F_NUM] = {
    1,      2,      3,      4,      4.9200, 5.9760, 6.9360, 7.8960, 8.8560, 9.8160, 10.680, 11.640, 12.504, 13.464, 14.328,
    15.192, 16.056, 16.824, 17.688, 18.456, 19.320, 20.088, 20.856, 21.528, 22.296, 22.968, 23.640, 24.408, 24.984, 25.656,
    26.328, 26.904, 27.480, 28.056, 28.632, 29.208, 29.784, 30.264, 30.840, 31.320, 31.800, 32.280, 32.760, 33.240, 33.624,
    34.104, 34.488, 34.872, 35.352, 35.736, 36.120, 36.504, 36.792, 37.176, 37.560, 37.944, 38.232, 38.520, 38.904, 39.192};

const double NotchFilter::ANGEL_FREQUENCY[NotchFilter::F_NUM] = {1,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
                                                                 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
                                                                 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60};

double NotchFilter::calCenterFrequency(double qd, double k)
{
    double F = fabs(qd) / M_PI * k;
    if (F < BASIC_FREQUENCY[0] || F > BASIC_FREQUENCY[55])
    {
        F = -1;
    }
    return F;
}

double NotchFilter::interp(const double* X, const double* Y, double x)
{
    for (int i = 0; i < F_NUM; i++)
    {
        if (x < X[i])
        {
            return (x - X[i - 1]) / (X[i] - X[i - 1]) * (Y[i] - Y[i - 1]) + Y[i - 1];
        }
    }
    return Y[F_NUM - 1];
}

// F : 陷波中心频率，depth : 陷波深度, Fs : 采样频率
NotchFilter::ADaptNotchPara NotchFilter::calNotchPara(double F, double depth, double Fs)
{
    double wn = interp(BASIC_FREQUENCY, ANGEL_FREQUENCY, F);
    wn = wn * 2.0 * M_PI;               // 陷波中心频率
    double B = wn / 10.0 * 2.0 * M_PI;  // 陷波带宽
    double Ts = 1 / Fs;

    double k1 = sqrt((-sqrt(B * B / (wn * wn) + 1) + 1) / (4 * depth * depth - 2));
    double k2 = depth * k1;
    double a1 = wn * wn * Ts * Ts + 4 * k1 * wn * Ts + 4;
    double a2 = 2 * wn * wn * Ts * Ts - 8;
    double a3 = wn * wn * Ts * Ts - 4 * Ts * k1 * wn + 4;
    double b1 = wn * wn * Ts * Ts + 4 * Ts * k2 * wn + 4;
    double b2 = 2 * wn * wn * Ts * Ts - 8;
    double b3 = wn * wn * Ts * Ts - 4 * Ts * k2 * wn + 4;

    ADaptNotchPara res;
    res.k3 = b3 / a1;
    res.k2 = b2 / a1;
    res.k1 = b1 / a1;
    res.m1 = -a2 / a1;
    res.m2 = -a3 / a1;

    return res;
}

// qd : 指令速度，input ：信号，Fs : 采样频率，k ：减速比，depth ：陷波深度，i : 关节数
double NotchFilter::do_filter(double qd, double input, double Fs, double k, double depth)
{
    double output = 0;
    // filter parameters
    double F = calCenterFrequency(qd, k);
    if (F < 0)
    {
        output = input;
        // update
        y_n2 = y_n1;
        y_n1 = output;
        x_n2 = x_n1;
        x_n1 = input;
    }
    else
    {
        ADaptNotchPara para = calNotchPara(F, depth, Fs);

        double k1 = para.k1;
        double k2 = para.k2;
        double k3 = para.k3;
        double m1 = para.m1;
        double m2 = para.m2;
        double temp1 = k1 * x_n1;
        double temp2 = k2 * x_n2;
        output = k1 * input + k2 * x_n1 + k3 * x_n2 + m1 * y_n1 + m2 * y_n2;
    }

    // update
    y_n2 = y_n1;
    y_n1 = output;
    x_n2 = x_n1;
    x_n1 = input;

    return output;
}
